#include <cstdio>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/video.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;


#include "targettrack.h"

TargetTrack::TargetTrack():m_track_imgs(40)
{
    sem_init(&this->m_frame_mutex, 0, 1);
    sem_init(&this->m_track_mutex, 0, 1);
    this->m_status = false;
    this->m_track_new = true;
}

TargetTrack::~TargetTrack(){

    printf("track closed\t");
    TimeStamp::now()->print();
}

bool TargetTrack::getImage(Mat &img, TimeStamp &ts){
    sem_wait(&this->m_frame_mutex);
    if(!this->m_cur.empty()){
        this->m_cur.copyTo(img);
        ts = this->m_cur_ts;
        sem_post(&this->m_frame_mutex);
        return true;
    }else{
        sem_post(&this->m_frame_mutex);
        return false;
    }

}

void TargetTrack::setCamera(CameraDaemon* cam){
    this->m_cam = cam;
}

void TargetTrack::nextFrame()
{
    Mat temp;
    TimeStamp temp_ts;
    sem_wait(&this->m_frame_mutex);
    if(this->m_cam->getImage(temp, temp_ts)){
        if(!this->m_cur.empty()){
            this->m_cur.copyTo(this->m_prev);
            this->m_prev_ts = this->m_cur_ts;
        }
        temp.copyTo(this->m_cur);
        this->m_cur_ts = temp_ts;
    }
    sem_post(&this->m_frame_mutex);
    temp.release();
}

static void getMidPoint(Point2f pt1, Point2f pt2, Point2f &ptm){
    ptm.x = pt1.x + pt2.x;
    ptm.y = pt1.y + pt2.y;
    ptm.x /= 2;
    ptm.y /= 2;
}







bool TargetTrack::getTrackImage(Mat &img, TimeStamp &ts){
    sem_wait(&this->m_track_mutex);
    bool ret;
    ret = this->getImage(img, ts);
    if(ret){
        this->m_track_imgs.enqueue(ts.toInt(), img);
     }

    sem_post(&this->m_track_mutex);

    return ret;
}

static void drawTarget(Mat &img, Point2f center, Rect area){
    Scalar blue(255, 0 , 0);
    Scalar red(0, 0, 255);
    Scalar green(0, 255, 0);
    circle(img, center, 3, red, 2);
    rectangle(img, area, blue, 2);
    printf("(%3f,%3f)\n", center.x, center.y);
}

bool TargetTrack::getTrackResult(Mat &img){
    bool ret;
    sem_wait(&this->m_frame_mutex);
    ret = !this->m_cur.empty();
    if(ret){
        this->m_cur.copyTo(img);
        if(this->m_status){
            drawTarget(img, this->m_obj_center, this->m_search_area) ;
        }
    }
    sem_post(&this->m_frame_mutex);
    return ret;
}

void TargetTrack::getTrackResult(TimeStamp &ts, Rect &area, Point2f &center){
    ts = this->m_cur_ts;
    area = this->m_search_area;
    center = this->m_obj_center;
}



void TargetTrack::startTarck(TimeStamp &ts, int x1, int y1, int x2, int y2){
    sem_wait(&this->m_track_mutex);


    printf("req\t");
    ts.print();
    printf("prev\t");
    this->m_prev_ts.print();

    if(this->m_track_imgs.find(ts.toInt(), &this->m_track_img)){
        this->m_track_img_ts = ts;
        printf("found img\n");
        this->m_cur_keypoints.clear();
        this->m_track_new = true;
        this->m_top_left = Point2f(x1, y1);
        this->m_bottom_right = Point2f(x2, y2);
        this->m_search_area = Rect(this->m_top_left, this->m_bottom_right);
        getMidPoint(this->m_top_left, this->m_bottom_right, this->m_obj_center);
        this->m_velocity_x = 0;
        this->m_velocity_y = 0;
//        this->setTarget(this->m_top_left, this->m_bottom_right);
        this->m_status = true;
    }else{

    }

//    if(this->m_track_img_ts == ts){
//        this->m_cur_keypoints.clear();
//        this->m_track_new = true;
//        this->m_top_left = Point2f(x1, y1);
//        this->m_bottom_right = Point2f(x2, y2);
//        this->setTarget(this->m_top_left, this->m_bottom_right);
//        this->m_status = true;
//    }else{


////        this->m_track_new = true;
////        this->m_top_left = Point2f(x1, y1);
////        this->m_bottom_right = Point2f(x2, y2);
////        this->setTarget(this->m_top_left, this->m_bottom_right);
////        this->m_status = true;
//    }
    sem_post(&this->m_track_mutex);
}

bool TargetTrack::selectMask(Mat mask, Rect area, Mat& out, Point2f* center){
    Mat temp = Mat::zeros(mask.size(), CV_8U);
    temp(area).setTo(255);
    bitwise_and(mask, temp, out);
    Moments m = moments(out, true);
    if(m.m00 < 1){
        return false;
    }else{
        if(center != NULL){
            center->x = m.m10 / m.m00;
            center->y = m.m01 / m.m00;
        }
    }
    return true;
}

static inline void setRectRange(Rect &rect, int width, int height){
    if (rect.x < 0 ){
        rect.x = 0;
    }else if (rect.x > width - 1){
        rect.x = width - 1;
    }
    if (rect.y < 0){
        rect.y = 0;
    }else if (rect.y > height - 1){
        rect.y = height - 1;
    }
    if (rect.width <= 0){
        rect.width = 1;
    }else if(rect.x + rect.width > width){
        rect.width = width - rect.x;
    }
    if(rect.height <= 0){
        rect.height = 1;
    }else if(rect.height + rect.y > height){
        rect.height = height - rect.y;
    }
}

